#include "APPAlgorithm.h"
#include "../RSTController.h"
#include "Utils.h"
#include <sstream>

#include <numeric>

namespace RafMat
{
namespace SimEngine
{

using namespace std;
using namespace SimInterface;

const string APPAlgorithm::CONF_AMPOLY("ampoly");
const string APPAlgorithm::CONF_ADEGREE("nda");
const string APPAlgorithm::CONF_BPOLY("bpoly");
const string APPAlgorithm::CONF_BDEGREE("ndb");
const string APPAlgorithm::CONF_KDELAY("kdly");

const double APPAlgorithm::ESTB_TOLERANCE = 1e-2;

// Do debugowania:
//static void printVect1(ostream& o, const std::string n, const RmnkSolver::Matrix& m)
//{
//    o << n << ":[ ";
//    RmnkSolver::Matrix::const_iterator1 it;
//    for(it =m.begin1(); it != m.end1(); ++it)
//        o << *it << ' ';
//    o << "]" << endl;
//}

APPAlgorithm::APPAlgorithm(RSTController& owner)
    : AdaptiveAlgorithmBase(owner, "app", "APP"),
       m_k(1), m_ndA(1), m_ndB(0)
{
    m_AmPoly = Polynomial(1, 1);
    m_AmPoly[1] = -0.7;

    setKDelay(1);
}

void APPAlgorithm::adapt(double ctrlVar, double processVar, double setPoint)
{
    (void) setPoint; // niewykorzystane

    double gamma;

    // Identyfikacja modelu:
    m_modelWrmnk->identify(ctrlVar, processVar);
    if(!m_adaptDelay)
    {
        m_bpoly = m_modelWrmnk->getEstimatedB();
    }
    else m_adaptDelay--;

    //std::cout << m_bpoly << " " << m_modelWrmnk->getEstimatedA() << std::endl;

    // Najpierw wkładamy próbki y(i) do pamięci gdyż potrzebujemy wartości bierzących.
    m_ymem.push_front(processVar);
    m_ymem.pop_back();             // usunięcie ostatniej próbki.
    m_umem.push_front(ctrlVar);
    m_umem.pop_back();             // usunięcie ostatniej próbki.

    // Na chwilą obecną:
    // m_ymem zawiera y(i) .. y(i-k-dSB) lub y(i-dAm) - zależy co ma większy rząd
    // m_umem zawiera u(i) .. u(i-k-dRB)

    // Obliczenie próbek uf(i-k) = B*u(i-k) - zaaplikowanie średniej ruchomej
    for(unsigned j = 0; j <= m_ndR; j++)
    {
        // Definicja iteratorów:
        vector<double>::iterator
                bpoly_it = m_bpoly.begin(),         // zaczynamy od b0
                bpoly_last_it = m_bpoly.end();      // kończymy na bdB
        deque<double>::iterator
                umem_it = m_umem.begin() + m_k + j; // zaczynamy od u(i-k-j)

        m_ufmem[j] = inner_product(bpoly_it, bpoly_last_it, umem_it, double(0), plus<double>(), multiplies<double>());
    }

    // Obliczenie próbek yf(i) = B*y(i) - zaaplikowanie średniej ruchomej
    for(unsigned j = 0; j <= m_ndS; j++)
    {
        // Definicja iteratorów:
        vector<double>::iterator
                bpoly_it = m_bpoly.begin(),         // zaczynamy od b0
                bpoly_last_it = m_bpoly.end();      // kończymy na bdB
        deque<double>::iterator
                ymem_it = m_ymem.begin() + m_k + j; // zaczynamy od y(i-k-j)

        m_yfmem[j] = inner_product(bpoly_it, bpoly_last_it, ymem_it, double(0), plus<double>(), multiplies<double>());
    }

    // Definicja iteratorów:
    vector<double>::iterator
            ampoly_it = m_AmPoly.begin(),           // zaczynamy od am0
            ampoly_last_it = m_AmPoly.end();
    deque<double>::iterator
            ymem_it = m_ymem.begin();               // zaczynamy od y(i)

    // gamma(i) = Am0*y(i) + Am1*y(i-1) + ... + AmdAm*y(i-dAm)
    gamma = inner_product(ampoly_it, ampoly_last_it, ymem_it, double(0), plus<double>(), multiplies<double>());

    // Budujemy wektor próbek dla identyfikacji

    copy(m_ufmem.begin(), m_ufmem.end(), m_phi.begin1());
    copy(m_yfmem.begin(), m_yfmem.end(), m_phi.begin1() + m_ndR + 1);

    //printVect1(cout, "Przed phi ", m_phi);
    //printVect1(cout, "Przed [R S] ", m_theta);

    // identyfikuj regulator
    m_theta = m_wrmnk->identify(m_theta, m_phi, gamma);

    //printVect1(cout, "Po [R S] ", m_theta);

    // Ekstrakcja wielomianów RB i SB:
    copy(m_theta.begin1(), m_theta.begin1() + m_ndR + 1, m_r.begin());
    copy(m_theta.begin1() + m_ndR+1, m_theta.end1(), m_s.begin());

    // Wyliczenie wielomianu T na bazie wzmocnienia T = Km = Am(1) / B(1)
    // Korzystamy z faktu, że Polynomial jest zaprojektowany jako
    // obiekt funkcyjny.
    m_t = m_AmPoly(1) / m_bpoly(1);

    // Wymuszenie r0 = 1
    double reductionRatio = m_r[0];

    //cout << "r0: " << reductionRatio << endl;

    if(reductionRatio > 1e-3)
    {
        m_s /= reductionRatio;
        m_t /= reductionRatio;
        m_r /= reductionRatio;
    }
    else
    {
        m_r[0] = 1;
    }

    // finito bro
}

//void APPAlgorithm::adapt(double ctrlVar, double processVar, double setPoint)
//{
//    (void) setPoint; // niewykorzystane

//    double gamma;

//    cout << endl;

//    // Najpierw wkładamy próbki y(i) do pamięci gdyż potrzebujemy wartości bierzących.
//    // usunięcie ostatniej próbki.
//    m_ymem.push_front(processVar);
//    m_ymem.pop_back();             // usunięcie ostatniej próbki.
//    m_umem.push_front(ctrlVar);
//    m_umem.pop_back();

//    // Na chwilą obecną:
//    // m_ymem zawiera y(i) .. y(i-k-dSB) lub y(i-dAm) - zależy co ma większy rząd
//    // m_umem zawiera u(i) .. u(i-k-dRB)

//    // Definicja iteratorów:
//    vector<double>::iterator
//            ampoly_it = m_AmPoly.begin(),                    // zaczynamy od am0
//            ampoly_last_it = m_AmPoly.end();
//    deque<double>::iterator
//            ymem_it = m_ymem.begin();                        // zaczynamy od y(i)

//    // gamma(i) = Am0*y(i) + Am1*y(i-1) + ... + AmdAm*y(i-dAm)
//    gamma = inner_product(ampoly_it, ampoly_last_it, ymem_it, double(0), plus<double>(), multiplies<double>());

//    // Budujemy wektor próbek dla identyfikacji

//    copy(m_umem.begin() + m_k, m_umem.end(), m_phi.begin1());
//    copy(m_ymem.begin() + m_k, m_ymem.begin() + m_k + m_ndSB + 1, m_phi.begin1() + m_ndRB + 1);

//    printVect1(cout, "Przed phi ", m_phi);
//    printVect1(cout, "Przed [RB SB] ", m_theta);

//    // identyfikuj regulator
//    m_theta = m_wrmnk->identify(m_theta, m_phi, gamma);

//    printVect1(cout, "Po [RB SB] ", m_theta);

//    // Ekstrakcja wielomianów RB i SB:
//    copy(m_theta.begin1(), m_theta.begin1() + m_ndRB + 1, m_rb.begin());
//    copy(m_theta.begin1() + m_ndRB+1, m_theta.end1(), m_sb.begin());

//    m_r = m_rb / m_bpoly;
//    m_s = m_sb / m_bpoly;
//    m_r.forceDegree(m_ndR);
//    m_s.forceDegree(m_ndS);

//    // Wyliczenie wielomianu T na bazie wzmocnienia T = Km = Am(1) / B(1)
//    // Korzystamy z faktu, że Polynomial jest zaprojektowany jako
//    // obiekt funkcyjny.
//    m_t = m_AmPoly(1) / m_bpoly(1);

//    cout << "Divide check: " << endl;
//    cout << "B = " << m_bpoly << endl;
//    cout << "RB = " << m_rb << " R = " << m_r << endl;
//    cout << "RB - R*B = " << m_rb - m_r * m_bpoly << endl;
//    cout << "SB = " << m_sb << " S = " << m_s << endl;
//    cout << "SB - S*B = " << m_sb - m_s * m_bpoly << endl;

//    // Ograniczenie wartości współczynników
////    m_r.limitCoeffs(1e3);
////    m_s.limitCoeffs(1e3);
////    m_t.limitCoeffs(1e3);

//    // Wymuszenie r0 = 1
// //   m_r[0] = 1;
//    double reductionRatio = m_r[0];

//    cout << "r0: " << reductionRatio << endl;

//    if(reductionRatio > 1e-3)
//    {
//        m_s /= reductionRatio;
//        m_t /= reductionRatio;
//        m_r /= reductionRatio;
//    }
//    else
//    {
//        m_r[0] = 1;
//    }

//    // finito bro
//}

void APPAlgorithm::reset()
{
    resizeBuffers();
    resetWrnmnk();
    cleanBuffers();

    // Wstępna inicjalizacja wielomianów
    m_r = Polynomial(m_ndR, 0);
    m_s = Polynomial(m_ndS, 0);
    m_r[0] = 1;
    m_t = Polynomial(0, 1);

    m_adaptDelay = 25;
    m_bpoly = Polynomial(m_ndB, 0);
    m_bpoly[0] = 100;

    // Początkowa wartość rb i sb nie jest istotna, jedynie rząd.
    //m_rb = Polynomial(m_ndRB, 0);
    //m_sb = Polynomial(m_ndSB, 0);
}

void APPAlgorithm::resetWrnmnk()
{
    // A := R,
    // B := S

    // Całkowity rząd wektora theta i innych
    int wrmnkRank = m_ndR + m_ndS + 2;

    m_wrmnk.reset(new RmnkSolver(wrmnkRank, 0.95));

    m_theta = RmnkSolver::Matrix(wrmnkRank, 1);

    // Wektor theta zainicjalizowany jako [1 0.1 0.1 ... ]
    fill(m_theta.begin1(), m_theta.end1(), 0);
    m_theta(0, 0) = 1;

    // Wektor fi zainicjalizowany zerami
    m_phi = RmnkSolver::Matrix(wrmnkRank, 1);
    fill(m_phi.begin1(), m_phi.end1(), 0);


    m_modelWrmnk.reset(new WrmnkARXWrapper(m_ndA, m_ndB, m_k));
}

void APPAlgorithm::resizeBuffers()
{
    // bufory muszą być dopasowane do wielomianów RB i SB:
    //m_ndRB = m_ndR + m_ndB;
    //m_ndSB = m_ndS + m_ndB;

    // Pamięć potrzebna na próbki od y(i) do y(i-k-dSB) lub y(i-dAm)
    int degree = max(m_ndS + m_ndB + m_k + 1, m_AmPoly.degree() + 1);
    m_ymem.resize(degree);

    // Pamięć potrzebna na próbki od u(i) do u(i-k-dRB)
    m_umem.resize(m_ndR + m_ndB + m_k + 1);

    m_ufmem.resize(m_ndR + 1);
    m_yfmem.resize(m_ndS + 1);

}

void APPAlgorithm::cleanBuffers()
{
    fill(m_ymem.begin(), m_ymem.end(), 0);
    fill(m_umem.begin(), m_umem.end(), 0);
}

//
// Konfiguracja parametrów z poziomu GIU
//

void APPAlgorithm::setAMPoly(const Polynomial& amPoly)
{
    m_AmPoly = amPoly;
    resizeBuffers();
}

//void APPAlgorithm::setRDegree(unsigned int ndR)
//{
//    int ndB = ndR - m_k + 1;

//    if(ndB < 0)
//        throw iconf_error("Dla danego ndR nie można dopasować dB >= 0.");

//    m_ndR = ndR;
//    m_ndB = ndB;

//    reset();
//    getOwner().resizeBuffersIfNecessary();
//}


//void APPAlgorithm::setSDegree(unsigned int ndS)
//{
//    m_ndS = ndS;

//    reset();
//    getOwner().resizeBuffersIfNecessary();
//}

void APPAlgorithm::setADegree(unsigned int ndA)
{
    int ndS = max<int>(static_cast<int>(ndA) - 1,
                  static_cast<int>(m_AmPoly.degree()) - m_k - m_ndB);

    if(ndS < 0)
        throw iconf_error("Dla obecnego rzędu dA uzyskuje się ujemne dS.");

    m_ndS = ndS;
    m_ndA = ndA;

    reset();
    getOwner().resizeBuffersIfNecessary();
}

void APPAlgorithm::setBPoly(const Polynomial& bpoly)
{
    int ndB = bpoly.degree();
    int ndR = ndB + m_k - 1;

    m_ndR = ndR;
    m_ndB = ndB;

    m_bpoly = bpoly;

    // dS zależy od dA i dB, więc wywołaj setADegree żeby wymusić przeliczenia.
    setADegree(m_ndA);
}

void APPAlgorithm::setBDegree(unsigned int ndB)
{
    int ndR = ndB + m_k - 1;

    m_ndR = ndR;
    m_ndB = ndB;

    // dS zależy od dA i dB, więc wywołaj setADegree żeby wymusić przeliczenia.
    setADegree(m_ndA);
}

void APPAlgorithm::setKDelay(unsigned int k)
{
    m_k = k;

    // Wymuś przeliczenie pozostałych parametrów
    setBPoly(m_bpoly);
}

bool APPAlgorithm::setParameter(const string& param, const string& value) throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string tvalue = Utils::trimWhiteSpaces(value);

    // Strumień danych dla łańcucha argumentu.
    istringstream istr(tvalue);

    double tmpVal;
    istr >> tmpVal;

    bool success = true;

    if(tparam == CONF_AMPOLY)
    {
        Polynomial poly = Polynomial(Utils::convertStrToVector(tvalue, /* out */ success));
        if(success)
        {
            setAMPoly(poly);
        }
    }
    else if(tparam == CONF_ADEGREE)
    {
        if(!istr.bad())
        {
            setADegree(tmpVal);
        }
    }
    else if(tparam == CONF_BPOLY)
    {
        Polynomial poly = Polynomial(Utils::convertStrToVector(tvalue, /* out */ success));
        if(success)
        {
            setBPoly(poly);
        }
    }
    else if(tparam == CONF_BDEGREE)
    {
        if(!istr.bad())
        {
            setBDegree(tmpVal);
        }
    }
    else if(tparam == CONF_KDELAY)
    {
        if(!istr.bad())
        {
            setKDelay(tmpVal);
        }
    }
    else
        throw iconf_error::unrecognizedParam(*this, tparam);

    if(istr.bad() || !success)
        throw iconf_error::syntaxError(*this, tvalue);

    return true;
}

std::string APPAlgorithm::getParameter(const std::string& param) const throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string outStr;

    // Strumień danych dla łańcucha argumentu.
    ostringstream ostr(outStr);

    if(tparam == CONF_AMPOLY)
    {
        ostr << m_AmPoly;
    }
    else if(tparam == CONF_ADEGREE)
    {
        ostr << m_ndA;
    }
    else if(tparam == CONF_BPOLY)
    {
        ostr << m_bpoly;
    }
    else if(tparam == CONF_BDEGREE)
    {
        ostr << m_ndB;
    }
    else if(tparam == CONF_KDELAY)
    {
        ostr << m_k;
    }
    else
        return AdaptiveAlgorithmBase::getParameter(tparam);

    return ostr.str();
}

SimFile::ConfigWriter& APPAlgorithm::dumpParameters(SimFile::ConfigWriter& o) const
{
    o.pushParam("adpt::" + CONF_AMPOLY, m_AmPoly);
    o.pushParam("adpt::" + CONF_ADEGREE, m_ndA);
    o.pushParam("adpt::" + CONF_BPOLY, m_bpoly);
    o.pushParam("adpt::" + CONF_BDEGREE, m_ndB);
    o.pushParam("adpt::" + CONF_KDELAY, m_k);
    return o;
}

}
}
